﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;


public partial struct ZMatrix4X4 : ISerializeStruct
{
    [NonSerialized]
    private FixedFloat[,] m_Pts;
    private FixedFloat[,] pts
    {
        get
        {
            if (m_Pts == null || m_Pts.GetLength(0) < 4 || m_Pts.GetLength(1) < 4)
            {
                m_Pts = new FixedFloat[4, 4];

                m_Pts[0, 0] = m_Col1.x;
                m_Pts[0, 1] = m_Col2.x;
                m_Pts[0, 2] = m_Col3.x;
                m_Pts[0, 3] = m_Col4.x;

                m_Pts[1, 0] = m_Col1.y;
                m_Pts[1, 1] = m_Col2.y;
                m_Pts[1, 2] = m_Col3.y;
                m_Pts[1, 3] = m_Col4.y;

                m_Pts[2, 0] = m_Col1.z;
                m_Pts[2, 1] = m_Col2.z;
                m_Pts[2, 2] = m_Col3.z;
                m_Pts[2, 3] = m_Col4.z;

                m_Pts[3, 0] = m_Col1.w;
                m_Pts[3, 1] = m_Col2.w;
                m_Pts[3, 2] = m_Col3.w;
                m_Pts[3, 3] = m_Col4.w;
            }
            return m_Pts;
        }
        set { m_Pts = value; }
    }

    FixedVec4 m_Col1 ;
    public FixedVec4 col1 => m_Col1;
    FixedVec4 m_Col2;
    public FixedVec4 col2 => m_Col2;
    FixedVec4 m_Col3;
    public FixedVec4 col3 => m_Col3;
    FixedVec4 m_Col4;
    public FixedVec4 col4 => m_Col4;

    public FixedFloat[,] matrixArr => m_Pts;

    public static ZMatrix4X4 identity
    {
        get
        {
            ZMatrix4X4 res = new ZMatrix4X4();
            res[1, 1] = res[2, 2] = res[3, 3] = res[4, 4] = 1;
            return res;
        }
    }

    public ZMatrix4X4(FixedFloat[,] matrixArr)
    {
        this.m_Pts = matrixArr;

        if (m_Pts == null)
            m_Pts = new FixedFloat[4, 4];

        m_Col1.x = m_Pts[0, 0];
        m_Col2.x = m_Pts[0, 1];
        m_Col3.x = m_Pts[0, 2];
        m_Col4.x = m_Pts[0, 3];

        m_Col1.y = m_Pts[1, 0];
        m_Col2.y = m_Pts[1, 1];
        m_Col3.y = m_Pts[1, 2];
        m_Col4.y = m_Pts[1, 3];

        m_Col1.z = m_Pts[2, 0];
        m_Col2.z = m_Pts[2, 1];
        m_Col3.z = m_Pts[2, 2];
        m_Col4.z = m_Pts[2, 3];

        m_Col1.w = m_Pts[3, 0];
        m_Col2.w = m_Pts[3, 1];
        m_Col3.w = m_Pts[3, 2];
        m_Col4.w = m_Pts[3, 3];
    }
    


    public FixedFloat this[int i, int j]
    {
        get
        {
            int xIndex = i - 1;
            if (xIndex < 0 || xIndex >= this.pts.GetLength(0))
            {
                GameDebug.LogError($"xIndex:{xIndex} this.pts.GetLength(0):{ this.pts.GetLength(0)}");
                return 0;
            }
            int yIndex = j - 1;
            if (yIndex < 0 || yIndex >= this.pts.GetLength(1))
            {
                GameDebug.LogError($"yIndex:{yIndex} this.pts.GetLength(1):{ this.pts.GetLength(1)}");
                return 0;
            }
            return pts[xIndex, yIndex];
        }
        set
        {
            int xIndex = i - 1;
            if (xIndex < 0 || xIndex >= this.pts.GetLength(0))
            {
                GameDebug.LogError($"xIndex:{xIndex} this.pts.GetLength(0):{ this.pts.GetLength(0)}");
                return;
            }
            int yIndex = j - 1;
            if (yIndex < 0 || yIndex >= this.pts.GetLength(1))
            {
                GameDebug.LogError($"yIndex:{yIndex} this.pts.GetLength(1):{ this.pts.GetLength(1)}");
                return;
            }
            this.pts[xIndex, yIndex] = value;

            UpdataData(xIndex, yIndex, value);
        }
    }

    void UpdataData(int rowIndex,int colIndex,FixedFloat value)
    {
        if(colIndex == 0)
        {
            switch (rowIndex)
            {
                case 0: m_Col1.x = value; break;
                case 1: m_Col1.y = value; break;
                case 2: m_Col1.z = value; break;
                case 3: m_Col1.w = value; break;
            }
        }
        else if (colIndex == 1)
        {
            switch (rowIndex)
            {
                case 0: m_Col2.x = value; break;
                case 1: m_Col2.y = value; break;
                case 2: m_Col2.z = value; break;
                case 3: m_Col2.w = value; break;
            }
        }
        else if (colIndex == 2)
        {
            switch (rowIndex)
            {
                case 0: m_Col3.x = value; break;
                case 1: m_Col3.y = value; break;
                case 2: m_Col3.z = value; break;
                case 3: m_Col3.w = value; break;
            }
        }
        else if (colIndex == 3)
        {
            switch (rowIndex)
            {
                case 0: m_Col4.x = value; break;
                case 1: m_Col4.y = value; break;
                case 2: m_Col4.z = value; break;
                case 3: m_Col4.w = value; break;
            }
        }
    }
    public static ZMatrix4X4 operator *(ZMatrix4X4 a, ZMatrix4X4 b)
    {
        return a.Mul(b);
    }
    public static FixedVec4 operator *(ZMatrix4X4 a, FixedVec4 b)
    {
        return a.Mul(b);
    }
    public static FixedVec3 operator *(ZMatrix4X4 a, FixedVec3 b)
    {
        return a.Mul(b);
    }
    public ZMatrix4X4 Mul(ZMatrix4X4 m)
    {
        ZMatrix4X4 newMatrix = new ZMatrix4X4();
        for (int row = 1; row <= 4; row++)
        {
            for (int col = 1; col <= 4; col++)
            {
                for (int n = 1; n <= 4; n++)
                {
                    newMatrix[row, col] += this[row, n] * m[n, col];
                }
            }
        }

        return newMatrix;
    }

    /// <summary>
    /// 乘以4维向量
    /// </summary>
    /// <param name="v"></param>
    /// <returns></returns>
    public FixedVec4 Mul(FixedVec4 v)
    {
        FixedVec4 newV = new FixedVec4();

        newV.x = v.x * this[1, 1] + v.y * this[1, 2] + v.z * this[1, 3] + v.w * this[1, 4];
        newV.y = v.x * this[2, 1] + v.y * this[2, 2] + v.z * this[2, 3] + v.w * this[2, 4];
        newV.z = v.x * this[3, 1] + v.y * this[3, 2] + v.z * this[3, 3] + v.w * this[3, 4];
        newV.w = v.x * this[4, 1] + v.y * this[4, 2] + v.z * this[4, 3] + v.w * this[4, 4];

        return newV;
    }

    public static FixedVec4 Vec3ToVec4(FixedVec3 vec) => new FixedVec4(vec.x, vec.y, vec.z, 1);
    public static FixedVec3 Vec4ToVec3(FixedVec4 vec) => new FixedVec3(vec.x, vec.y, vec.z);

    /// <summary>
    /// 乘以三维向量
    /// </summary>
    /// <param name="v"></param>
    /// <returns></returns>
    public FixedVec3 Mul(FixedVec3 v)
    {
        FixedVec4 vec4 = Vec3ToVec4(v);
        FixedVec4 res = Mul(vec4);
        return Vec4ToVec3(res);
    }


    /// <summary>
    /// 行列式的值
    /// </summary>
    public FixedFloat determinant
    {
        get
        {
            return CalDeterminant(this.m_Pts);
        }
    }

    /// <summary>
    /// 逆矩阵
    /// </summary>
    public ZMatrix4X4 inverseMatrix
    {
        get
        {
            ZMatrix4X4 zMatrix = new ZMatrix4X4();
            zMatrix.pts = CalInverseMatrix(this.m_Pts);
            return zMatrix;
        }
    }

    /// <summary>
    /// 计算行列式的值
    /// </summary>
    /// <param name="zMatrix4"></param>
    /// <returns></returns>
    public static FixedFloat CalDeterminant(FixedFloat[,] matrixArr)
    {
        if (matrixArr.GetLength(0) != matrixArr.GetLength(1))
        {
            throw new System.ArgumentException("matrix row num != col num");
        }

        if (matrixArr.GetLength(0) == 0)
        {
            throw new System.ArgumentException("row num == 0");
        }

        int order = matrixArr.GetLength(0);

        if (order == 1)
            return matrixArr[0, 0];

        FixedFloat result = 0;
        for (int n = 0; n < order; n++)
        {
            //取第一行的各项与其代数余子式的积之和
            FixedFloat[,] cofactor = GetCofactor(matrixArr, 0, n);

            result += (n % 2 == 0 ? 1 : -1) * matrixArr[0, n] * CalDeterminant(cofactor);
        }

        return result;
    }

    /// <summary>
    /// 获取代数余子式 矩阵数组
    /// </summary>
    /// <param name="matrixArr"></param>
    /// <returns></returns>
    public static FixedFloat[,] GetCofactor(FixedFloat[,] matrixArr, int rowIndex, int colIndex)
    {
        if (matrixArr.GetLength(0) != matrixArr.GetLength(1))
        {
            throw new System.ArgumentException("matrix row num != col num");
        }

        if (matrixArr.GetLength(0) <= 0)
        {
            throw new System.ArgumentException("row num == 1");
        }

        int order = matrixArr.GetLength(0);

        if (order == 1)
            return new FixedFloat[0, 0];

        int cofactorOrder = order - 1;
        FixedFloat[,] cofactor = new FixedFloat[cofactorOrder, cofactorOrder];

        for (int i = 0; i < cofactorOrder; i++)
        {
            for (int j = 0; j < cofactorOrder; j++)
            {
                cofactor[i, j] = matrixArr[(i < rowIndex) ? i : i + 1, (j < colIndex) ? j : j + 1];
            }
        }

        return cofactor;
    }

    /// <summary>
    /// 计算伴随矩阵
    /// </summary>
    /// <param name="zMatrix"></param>
    /// <returns></returns>
    public static FixedFloat[,] GetAdjointMatrix(FixedFloat[,]  matrixArr)
    {
        if (!MatrixCommonCheck(matrixArr)) return null;
        
        int order = matrixArr.GetLength(0);

        if (order == 1)
            return matrixArr;

        FixedFloat[,] res = new FixedFloat[order, order];
            
        for(int i = 0; i < order; i++)
        {
            for (int j = 0; j < order; j++)
            {
                FixedFloat[,] cofactor = GetCofactor(matrixArr, i, j);
                int plusMinus = ((i + j) % 2 == 0 ? 1 : -1);
                res[j,i] = plusMinus * CalDeterminant(cofactor);

                //GameDebug.Log($"({i},{j})：代数余子式：\n{FixedArr2Float(cofactor).GetArrStr()}  的行列式值是 {plusMinus}*{CalDeterminant(cofactor)}  ");
            }
        }
        
        //GameDebug.Log($"伴随矩阵是：{FixedArr2Float(res)}");
        return res;
    }

    static float[,] FixedArr2Float(FixedFloat[,] res)
    {
        int order = res.GetLength(0);

        float[,] test = new float[order, order];
        for (int i = 0; i < order; i++)
        {
            for (int j = 0; j < order; j++)
            {
                test[i, j] = res[i, j];
            }
        }
        return test;
    }

    /// <summary>
    /// 计算矩阵的逆,行列式的倒数 x 伴随矩阵 
    /// </summary>
    /// <param name="matrixArr"></param>
    /// <returns></returns>
    public static FixedFloat[,] CalInverseMatrix(FixedFloat[,] matrixArr)
    {
        if (!MatrixCommonCheck(matrixArr)) return null;

        int order = matrixArr.GetLength(0);

        FixedFloat[,] res = new FixedFloat[order, order];

        //伴随矩阵
        FixedFloat[,] adJointMatrix = GetAdjointMatrix(matrixArr);

        for (int i = 0; i < order; i++)
        {
            for (int j = 0; j < order; j++)
            {
                res[i, j] = (1 / CalDeterminant(matrixArr)) * adJointMatrix[i, j];
            }
        }
        return res;
    }

    /// <summary>
    /// 计算转置矩阵
    /// </summary>
    /// <param name="matrixArr"></param>
    /// <returns></returns>
    public static FixedFloat[,] CalTransposeMatrix(FixedFloat[,] matrixArr)
    {
        if (!MatrixCommonCheck(matrixArr)) return null;

        int order = matrixArr.GetLength(0);

        FixedFloat[,] res = new FixedFloat[order, order];
        
        for (int i = 0; i < order; i++)
        {
            for (int j = 0; j < order; j++)
            {
                res[i, j] = matrixArr[i, j];
            }
        }
        return res;
    }

    static bool MatrixCommonCheck(FixedFloat[,] matrixArr)
    {
        if (matrixArr == null)
            return false;

        if (matrixArr.GetLength(0) != matrixArr.GetLength(1))
        {
            GameDebug.Log("matrix row num != col num");
            return false;
        }

        if (matrixArr.GetLength(0) <= 0)
        {
            GameDebug.Log("row num == 1");
            return false;
        }
        return true;
    }


    public override string ToString()
    {
        string str = "";

        str += this[1, 1] + ", " + this[1, 2] + ", " + this[1, 3] + ", " + this[1, 4] + "\n";
        str += this[2, 1] + ", " + this[2, 2] + ", " + this[2, 3] + ", " + this[2, 4] + "\n";
        str += this[3, 1] + ", " + this[3, 2] + ", " + this[3, 3] + ", " + this[3, 4] + "\n";
        str += this[4, 1] + ", " + this[4, 2] + ", " + this[4, 3] + ", " + this[4, 4] + "\n";
        
        return str;
    }

}
